#include "my_lib/follow.h"

#define PI  3.1415926f


extern uint8_t task_id;

PID_Type JiGuang_PID_X = {0};
PID_Type JiGuang_PID_Y = {0};
point_bias_t point_bias = {0};
uint8_t gPointFollowCtrl = 0;
uint8_t PointPID_Trans = 0;


/// @brief 设置pid参数
/// @param PID_P 
/// @param PID_I 
/// @param PID_D 
/// @param Limit 
/// @param BiasAllow 
static void SetJiGuangPID(float PID_P, float PID_I, float PID_D, float Limit, float BiasAllow)
{
   JiGuang_PID_X.Kp = PID_P;
   JiGuang_PID_X.Ki = PID_I;
   JiGuang_PID_X.Kd = PID_D;
   JiGuang_PID_X.OutLimit = Limit;
   JiGuang_PID_X.BiasAllow = BiasAllow;

   JiGuang_PID_Y.Kp = PID_P;
   JiGuang_PID_Y.Ki = PID_I;
   JiGuang_PID_Y.Kd = PID_D;
   JiGuang_PID_Y.OutLimit = Limit;
   JiGuang_PID_Y.BiasAllow = BiasAllow;
}

/// @brief 初始化
/// @param  
static void PointFollow_Init(void)
{
    SetJiGuangPID(PointKp, PointKi, PointKd, PointOutMax, PointBiasAllow);
}

static void PointPID_Ctrl(void)
{
    JiGuang_PID_X.Bias =point_bias.x_bias; //由坐标计算偏差距离
    JiGuang_PID_Y.Bias =point_bias.y_bias; 
    float bias_sqrt = sqrt(point_bias.x_bias*point_bias.x_bias+point_bias.y_bias*point_bias.y_bias);

    if(bias_sqrt > 100){
        JiGuang_PID_X.Kp = PointKp_START;  //初始移动pid
        JiGuang_PID_X.Kd = PointKd_START;  //初始移动pid
        JiGuang_PID_Y.Kp = PointKp_START;  //初始移动pid
        JiGuang_PID_Y.Kd = PointKd_START; 
    }   
    else{
        if(task_id == 3){
            JiGuang_PID_X.Kp = PointKp_FUN2;   //线上pid调控
            JiGuang_PID_X.Kd = PointKd_FUN2;   //线上pid调控
            JiGuang_PID_Y.Kp = PointKp_FUN2;   //线上pid调控
            JiGuang_PID_Y.Kd = PointKd_FUN2;
        }
        else if (task_id == 4)
        {
            JiGuang_PID_X.Kp = PointKp;   //线上pid调控
            JiGuang_PID_X.Kd = PointKd;   //线上pid调控
            JiGuang_PID_Y.Kp = PointKp;   //线上pid调控
            JiGuang_PID_Y.Kd = PointKd;
        }
        else{
            MY_LOGE("follow", "pid task_id[%d] error", task_id);
        }
     
    }
        
    JiGuang_PID_X.Output = JiGuang_PID_X.Kp*JiGuang_PID_X.Bias + JiGuang_PID_X.Kd*(JiGuang_PID_X.Bias-JiGuang_PID_X.LastBias);
    JiGuang_PID_Y.Output = JiGuang_PID_Y.Kp*JiGuang_PID_Y.Bias + JiGuang_PID_Y.Kd*(JiGuang_PID_Y.Bias-JiGuang_PID_Y.LastBias);

    JiGuang_PID_X.LastBias = JiGuang_PID_X.Bias;  //误差更新
    JiGuang_PID_Y.LastBias = JiGuang_PID_Y.Bias;  //误差更新
    //输出限幅
    if(JiGuang_PID_X.Output > JiGuang_PID_X.OutLimit)
    {
        JiGuang_PID_X.Output = JiGuang_PID_X.OutLimit;
    }else if(JiGuang_PID_X.Output < -JiGuang_PID_X.OutLimit)
    {
        JiGuang_PID_X.Output = -JiGuang_PID_X.OutLimit;
    }

    if(JiGuang_PID_Y.Output > JiGuang_PID_Y.OutLimit)
    {
        JiGuang_PID_Y.Output = JiGuang_PID_Y.OutLimit;
    }else if(JiGuang_PID_Y.Output < -JiGuang_PID_Y.OutLimit)
    {
        JiGuang_PID_Y.Output = -JiGuang_PID_Y.OutLimit;
    }

    float x_out, y_out;
    x_out = JiGuang_PID_X.Output;  //角度增量
    x_out = steers[HorizontalSteer].pre_angle + x_out; //目标控制角度
    y_out = JiGuang_PID_Y.Output;
    y_out = steers[VerticalSteer].pre_angle - y_out;
    
    if(x_out  < HORIZONTAL_STEER_MIN_ANGLE){
        x_out  = HORIZONTAL_STEER_MIN_ANGLE;
    }
    else if(x_out  > HORIZONTAL_STEER_MAX_ANGLE){
        x_out  = HORIZONTAL_STEER_MAX_ANGLE;
    }

    if(y_out > VERTICAL_STEER_MAX_ANGLE){
        y_out = VERTICAL_STEER_MAX_ANGLE;
    }
    else if(y_out < VERTICAL_STEER_MIN_ANGLE){
        y_out = VERTICAL_STEER_MIN_ANGLE;
    }

    SetSteerAngle(HorizontalSteer, x_out);
    SetSteerAngle(VerticalSteer, y_out);
}

/// @brief 是否启用点追踪功能
/// @param sta 
void IsPointfollow(uint8_t sta)
{
    point_bias.x_bias = 0;
    point_bias.y_bias = 0;
    
    gPointFollowCtrl = sta;
}

/// @brief 设置位置偏差
/// @param x_bias 水平偏差
/// @param y_bias 竖直偏差
void SetPointBias(double x_bias, double y_bias)
{
    point_bias.x_bias = x_bias;
    point_bias.y_bias = y_bias;
}


///////////////////////////////cmd test
static void IsPointPID_Trans(double param)
{
    MY_LOGI("point", "point pid trans:%d", (uint8_t)param);
    PointPID_Trans = (uint8_t)param;
}

static void PointFollowTest(double param)
{
    uint32_t value = param;
    uint8_t enable = value/1000000;
    value %= 1000000;
    uint8_t x_sign = value/100000;
    value %= 100000;
    int16_t x_bias = x_sign? -(value/1000):(value/1000);
    value %= 1000;
    uint8_t y_sign = value/100;
    value %= 100;
    int16_t y_bias = y_sign? (-value):value;

    MY_LOGI("pos adjust", "en[%d] x[%d] y[%d]", enable, x_bias, y_bias);

    IsPointfollow(enable);
    SetPointBias(x_bias, y_bias);
}
///////////////////////////////cmd test

void PointFollowTask(void* param)
{
    PointFollow_Init();
    CmdRegister("PFTS",
                "is enable point follow, like:1, en",
                IsPointPID_Trans);
    CmdRegister("PFTT",
                "point follow test, like:1023113, en[1] x[23] y[-13]",
                PointFollowTest);
    while (1)
    {
        if(gPointFollowCtrl){
            PointPID_Ctrl();
        }
        //temp test
//        if(PointPID_Trans)
//        {
//            float Info[2];
//            Info[0] = JiGuang_PID.Bias;
//            Info[1] = JiGuang_PID.Output;
//            VofaFloatSend(2,Info);
//        }   
        vTaskDelay(5);
    }
}


